#include "SVD.h"

#define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
#define MAX(a,b) ((a)>(b)?(a):(b))
#define MIN(a,b) ((a)<(b)?(a):(b))

#define MINRATIO 1.0e-6

inline double PYTHAG (double a, double b){
	double at = fabs(a), bt = fabs(b), ct, result;
 
	if (at > bt)     { ct = bt / at; result = at * sqrt(1.0 + ct * ct); }
	else if (bt > 0.0) { ct = at / bt; result = bt * sqrt(1.0 + ct * ct); }
	else result = 0.0;
	return(result);
}

int SVD::svdcmp(Matd& a, Vecd& w, Matd& v)
{
	int flag, i, its, j, jj, k, l, nm;
	int m = a.Rows(), n = a.Cols();
	double c, f, h, s, x, y, z;
	double anorm = 0.0, g = 0.0, scale = 0.0;
	double *rv1;

	//if (m < n){
	//	printf("#rows must be > #cols \n");
	//	return (0);
	//}
 
	rv1 = new double[n];//(double *) malloc (n*sizeof (double));
 	w.SetSize(n);
	v.SetSize(n,n);

 /* Householder reduction to bidiagonal form */
	for (i = 0; i < n; i++){
		/* left-hand reduction */
		l = i + 1;
		rv1[i] = scale * g;
	    g = s = scale = 0.0;
		if (i < m){
			for (k = i; k < m; k++)
		         scale += fabs (a[k][i]);
			if (scale){
				for (k = i; k < m; k++){
					a[k][i] = (a[k][i]/scale);
					s += (a[k][i] * a[k][i]);
				}
				f = a[i][i];
				g = -SIGN(sqrt (s), f);
				h = f * g - s;
				a[i][i] = (f - g);
				if (i != n - 1){
					for (j = l; j < n; j++){
						for (s = 0.0, k = i; k < m; k++)
							s += (a[k][i] * a[k][j]);
						f = s / h;
						for (k = i; k < m; k++)
							a[k][j] += (f * a[k][i]);
					}
				}
				for (k = i; k < m; k++)
					a[k][i] = (a[k][i]*scale);
			}
		}
		w[i] = (float)(scale * g);
 
		/* right-hand reduction */
		g = s = scale = 0.0;
		if (i < m && i != n - 1){
			for (k = l; k < n; k++)
				scale += fabs(a[i][k]);
			if (scale){
				for (k = l; k < n; k++){
					a[i][k] = (a[i][k]/scale);
					s += (a[i][k] * a[i][k]);
				}
				f = a[i][l];
				g = -SIGN(sqrt(s), f);
				h = f * g - s;
				a[i][l] = (f - g);
				for (k = l; k < n; k++)
					rv1[k] = a[i][k] / h;
				if (i != m - 1){
					for (j = l; j < m; j++){
						for (s = 0.0, k = l; k < n; k++)
							s += (a[j][k] * a[i][k]);
						for (k = l; k < n; k++)
							a[j][k] += (s * rv1[k]);
					}
				}
				for (k = l; k < n; k++)
					a[i][k] = (a[i][k]*scale);
			}
		}
		anorm = MAX(anorm, (fabs ((double)w[i]) + fabs(rv1[i])));
	}
 
   /* accumulate the right-hand transformation */
	for (i = n - 1; i >= 0; i--){
		if (i < n - 1){
			if (g){
				for (j = l; j < n; j++)
					v[j][i] = ((a[i][j] / a[i][l]) / g);
				/* double division to avoid underflow */
				for (j = l; j < n; j++){
					for (s = 0.0, k = l; k < n; k++)
						s += (a[i][k] * v[k][j]);
					for (k = l; k < n; k++)
						v[k][j] += (s * v[k][i]);
				}
			}
			for (j = l; j < n; j++)
				v[i][j] = v[j][i] = 0.0;
		}
		v[i][i] = 1.0;
		g = rv1[i];
		l = i;
	}
 
	/* accumulate the left-hand transformation */
	for (i = MIN(m,n) - 1; i >= 0; i--){
		l = i + 1;
		g = (double)w[i];
		if (i < n - 1){
			for (j = l; j < n; j++)
				a[i][j] = 0.0;
		}
		if (g){
			g = 1.0 / g;
			if (i != n - 1){
				for (j = l; j < n; j++){
					for (s = 0.0, k = l; k < m; k++)
						s += (a[k][i] * a[k][j]);
					f = (s / a[i][i]) * g;
					for (k = i; k < m; k++)
						a[k][j] += (f * a[k][i]);
				}
			}
			for (j = i; j < m; j++)
				a[j][i] = (a[j][i]*g);
		}
		else{
			for (j = i; j < m; j++)
				a[j][i] = 0.0;
		}
		++a[i][i];
	}
 
	/* diagonalize the bidiagonal form */
	for (k = n - 1; k >= 0; k--){               
		/* loop over singular values */
		for (its = 0; its < 30; its++){             
			/* loop over allowed iterations */
			flag = 1;
			for (l = k; l >= 0; l--){           
				/* test for splitting */
				nm = l - 1;
				if (fabs (rv1[l]) + anorm == anorm){
					flag = 0;
					break;
				}
				if (fabs ((double)w[nm]) + anorm == anorm)
					break;
			}
			if (flag){
				c = 0.0;
				s = 1.0;
				for (i = l; i <= k; i++){
					f = s * rv1[i];
					if (fabs(f) + anorm != anorm){
						g = (double)w[i];
						h = PYTHAG(f, g);
						w[i] = (float)h;
						h = 1.0 / h;
						c = g * h;
						s = (- f * h);
						for (j = 0; j < m; j++){
							y = a[j][nm];
							z = a[j][i];
							a[j][nm] = (y * c + z * s);
							a[j][i] = (z * c - y * s);
						}
					}
				}
			}
			z = (double)w[k];
			if (l == k){          
				/* convergence */
				if (z < 0.0){        
					/* make singular value nonnegative */
					w[k] = (float)(-z);
					for (j = 0; j < n; j++)
						v[j][k] = (-v[j][k]);
				}
				break;
			}
			if (its >= 30){
				//free (rv1);
				delete rv1;
				printf ("No convergence after 30,000! iterations \n");
				return(0);
			}
 
	       /* shift from bottom 2 x 2 minor */
			x = (double)w[l];
			nm = k - 1;
			y = (double)w[nm];
			g = rv1[nm];
			h = rv1[k];
			f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
			g = PYTHAG(f, 1.0);
			f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
 
	       /* next QR transformation */
			c = s = 1.0;
			for (j = l; j <= nm; j++){
				i = j + 1;
				g = rv1[i];
				y = (double)w[i];
				h = s * g;
				g = c * g;
				z = PYTHAG(f, h);
				rv1[j] = z;
				c = f / z;
				s = h / z;
				f = x * c + g * s;
				g = g * c - x * s;
				h = y * s;
				y = y * c;
				for (jj = 0; jj < n; jj++){
					x = (double)v[jj][j];
					z = (double)v[jj][i];
					v[jj][j] = (float)(x * c + z * s);
					v[jj][i] = (float)(z * c - x * s);
				}
				z = PYTHAG (f, h);
				w[j] = (float)z;
				if (z){
					z = 1.0 / z;
					c = f * z;
					s = h * z;
				}
				f = (c * g) + (s * y);
				x = (c * y) - (s * g);
				for (jj = 0; jj < m; jj++){
					y = a[jj][j];
					z = a[jj][i];
					a[jj][j] = (y * c + z * s);
					a[jj][i] = (z * c - y * s);
				}
			}
			rv1[l] = 0.0;
			rv1[k] = f;
			w[k] = (float)x;
		}
	}
	//free (rv1);
	delete rv1;
	return (1);
}

// Solves for x st Ax=b where A = uwvT (returned by svd) 
// x = v*invW*uT*b
void SVD::svdksb(Matd& u, Vecd& w, Matd& v, Vecd &b, Vecd &x){
	int m = u.Rows();
	int n = v.Cols();
	x = Vecd(n, vl_0);
	Vecd tmp(n, vl_0);
	// compute invw*uT*b
	for(int j=0; j<n; j++){
		if(w[j]){
			for(int i=0; i<m; i++) tmp[j] += u[i][j]*b[i];
			tmp[j]/=w[j];
		}
	}
	for(int j=0; j<n; j++){
		for(int k=0; k<n; k++) x[j] += v[j][k]*tmp[k];
	}
}

// returns the null space in a separate matrix
void SVD::nullspace(Vecd& w, Matd& v, Matd &vNull){
	int n = v.Cols();
	vector<int> index;
	for(int j=0; j<n; j++){
		if(w[j]==0) index.push_back(j); 
	}
	vNull = Matd(n, index.size(), vl_0);
	for(int j=0; j<index.size(); j++){
		for(int i=0; i<n; i++){
			vNull[i][j] = v[i][index[j]];
		}
	}
}

void SVD::cleandiag(Vecd &w){
	double wmax = 0.0;
	for(int i=0; i<w.Elts(); i++){
		if(w[i]>wmax) wmax = w[i];
	}
	double wmin = wmax * MINRATIO;
	for(int i=0; i<w.Elts(); i++){
		if(w[i]< wmin) w[i] = 0.0;
	}
}
